/*
 * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/* Adapted from Paul Furgale Schweizer Messer sm_timing*/

#include <math.h>
#include <stdio.h>
#include <algorithm>
#include <ostream>
#include <sstream>
#include <string>

#include <eth_trajectory_generation/timing.h>

namespace eth_trajectory_generation
{
namespace timing
{

Timing& Timing::Instance() {
  static Timing t;
  return t;
}

Timing::Timing() : max_tag_length_(0) {
}

Timing::~Timing() {
}

/* GetHandle() //{ */

// Static functions to query the timers:
size_t Timing::GetHandle(std::string const& tag) {
  // Search for an existing tag.
  map_t::iterator i = Instance().tag_map_.find(tag);
  if (i == Instance().tag_map_.end()) {
    // If it is not there, create a tag.
    size_t handle            = Instance().timers_.size();
    Instance().tag_map_[tag] = handle;
    Instance().timers_.push_back(TimerMapValue());
    // Track the maximum tag length to help printing a table of timing values
    // later.
    Instance().max_tag_length_ = std::max(Instance().max_tag_length_, tag.size());
    return handle;
  } else {
    return i->second;
  }
}

//}

/* GetTag() //{ */

std::string Timing::GetTag(size_t handle) {
  std::string tag;

  // Perform a linear search for the tag.
  for (typename map_t::value_type current_tag : Instance().tag_map_) {
    if (current_tag.second == handle) {
      return current_tag.first;
    }
  }
  return tag;
}

//}

/* Timer() //{ */

// Class functions used for timing.
Timer::Timer(size_t handle, bool constructStopped) : timing_(false), handle_(handle) {
  if (!constructStopped)
    Start();
}

//}

/* Timer() //{ */

Timer::Timer(std::string const& tag, bool constructStopped) : timing_(false), handle_(Timing::GetHandle(tag)) {
  if (!constructStopped)
    Start();
}

//}

/* ~Timer() //{ */

Timer::~Timer() {
  if (IsTiming())
    Stop();
}

//}

/* Start() //{ */

void Timer::Start() {
  timing_ = true;
  time_   = std::chrono::system_clock::now();
}

//}

/* Stop() //{ */

void Timer::Stop() {
  std::chrono::time_point<std::chrono::system_clock> now = std::chrono::system_clock::now();
  double                                             dt  = std::chrono::duration<double>(now - time_).count();

  Timing::Instance().AddTime(handle_, dt);
  timing_ = false;
}

//}

/* IsTiming() //{ */

bool Timer::IsTiming() const {
  return timing_;
}

//}

/* AddTime() //{ */

void Timing::AddTime(size_t handle, double seconds) {
  timers_[handle].acc_.Add(seconds);
}

//}

/* GetTotalSeconds() //{ */

double Timing::GetTotalSeconds(size_t handle) {
  return Instance().timers_[handle].acc_.Sum();
}

//}

/* GetTotalSeconds() //{ */

double Timing::GetTotalSeconds(std::string const& tag) {
  return GetTotalSeconds(GetHandle(tag));
}

//}

/* GetMeanSeconds() //{ */

double Timing::GetMeanSeconds(size_t handle) {
  return Instance().timers_[handle].acc_.Mean();
}

//}

/* GetMeanSeconds() //{ */

double Timing::GetMeanSeconds(std::string const& tag) {
  return GetMeanSeconds(GetHandle(tag));
}

//}

/* GetNumSamples() //{ */

size_t Timing::GetNumSamples(size_t handle) {
  return Instance().timers_[handle].acc_.TotalSamples();
}

//}

/* GetNumSamples() //{ */

size_t Timing::GetNumSamples(std::string const& tag) {
  return GetNumSamples(GetHandle(tag));
}

//}

/* GetVarianceSeconds() //{ */

double Timing::GetVarianceSeconds(size_t handle) {
  return Instance().timers_[handle].acc_.LazyVariance();
}

//}

/* GetVarianceSeconds() //{ */

double Timing::GetVarianceSeconds(std::string const& tag) {
  return GetVarianceSeconds(GetHandle(tag));
}

//}

/* GetMinSeconds() //{ */

double Timing::GetMinSeconds(size_t handle) {
  return Instance().timers_[handle].acc_.Min();
}

//}

/* GetMinSeconds() //{ */

double Timing::GetMinSeconds(std::string const& tag) {
  return GetMinSeconds(GetHandle(tag));
}

//}

/* GetMaxSeconds() //{ */

double Timing::GetMaxSeconds(size_t handle) {
  return Instance().timers_[handle].acc_.Max();
}

//}

/* GetMaxSeconds() //{ */

double Timing::GetMaxSeconds(std::string const& tag) {
  return GetMaxSeconds(GetHandle(tag));
}

//}

/* GetHz() //{ */

double Timing::GetHz(size_t handle) {
  return 1.0 / Instance().timers_[handle].acc_.RollingMean();
}

//}

/* GetHz() //{ */

double Timing::GetHz(std::string const& tag) {
  return GetHz(GetHandle(tag));
}

//}

/* SecondsToTimeString() //{ */

std::string Timing::SecondsToTimeString(double seconds) {
  char buffer[256];
  snprintf(buffer, sizeof(buffer), "%09.6f", seconds);
  return buffer;
}

//}

/* Print() //{ */

void Timing::Print(std::ostream& out) {
  map_t& tagMap = Instance().tag_map_;

  if (tagMap.empty()) {
    return;
  }

  out << "SM Timing\n";
  out << "-----------\n";
  for (typename map_t::value_type t : tagMap) {
    size_t i = t.second;
    out.width((std::streamsize)Instance().max_tag_length_);
    out.setf(std::ios::left, std::ios::adjustfield);
    out << t.first << "\t";
    out.width(7);

    out.setf(std::ios::right, std::ios::adjustfield);
    out << GetNumSamples(i) << "\t";
    if (GetNumSamples(i) > 0) {
      out << SecondsToTimeString(GetTotalSeconds(i)) << "\t";
      double meansec = GetMeanSeconds(i);
      double stddev  = sqrt(GetVarianceSeconds(i));
      out << "(" << SecondsToTimeString(meansec) << " +- ";
      out << SecondsToTimeString(stddev) << ")\t";

      double minsec = GetMinSeconds(i);
      double maxsec = GetMaxSeconds(i);

      // The min or max are out of bounds.
      out << "[" << SecondsToTimeString(minsec) << "," << SecondsToTimeString(maxsec) << "]";
    }
    out << std::endl;
  }
}

//}

/* Print() //{ */

std::string Timing::Print() {
  std::stringstream ss;
  Print(ss);
  return ss.str();
}

//}

/* Reset() //{ */

void Timing::Reset() {
  Instance().tag_map_.clear();
}

//}

}  // namespace timing
}  // namespace eth_trajectory_generation
